/*-----------------------------------------------------------------------------

 Copyright 2017 Hopsan Group

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

        http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.


 The full license is available in the file LICENSE.
 For details about the 'Hopsan Group' or information about Authors and
 Contributors see the HOPSANGROUP and AUTHORS files that are located in
 the Hopsan source code root directory.

-----------------------------------------------------------------------------*/

#ifndef SIGNALATTITUDECONTROL_HPP_INCLUDED
#define SIGNALATTITUDECONTROL_HPP_INCLUDED

#include <iostream>
#include "ComponentEssentials.h"
#include "ComponentUtilities.h"
#include "math.h"

//!
//! @file SignalAttitudeControl.hpp
//! @author Petter Krus <petter.krus@liu.se>
//  co-author/auditor **Not yet audited by a second person**
//! @date Tue 14 Apr 2015 16:48:36
//! @brief Attitude control unit for an aircraft
//! @ingroup SignalComponents
//!
//==This code has been autogenerated using Compgen==
//from 
/*{, C:, HopsanTrunk, componentLibraries, defaultLibrary, Signal, \
Control}/SignalControlAero.nb*/

using namespace hopsan;

class SignalAttitudeControl : public ComponentSignal
{
private:
     double Kphi;
     double Kphipsi;
     double Kelev;
     double Kdelev;
     double Krud;
     double Kdrud;
     double u1min;
     double u1max;
     double u2min;
     double u2max;
     double u3min;
     double u3max;
     double U0;
     int mNstep;
//==This code has been autogenerated using Compgen==
     //inputVariables
     double phiref;
     double thetaref;
     double psiref;
     double phimax;
     double phi;
     double theta;
     double psi;
     double beta;
     double Qb;
     double Rb;
     double Ub;
     //outputVariables
     double uaerL;
     double uaerR;
     double uelev;
     double urud;
     //LocalExpressions variables
     double Kv;
     double u1cmin;
     double u1cmax;
     double u2cmin;
     double u2cmax;
     double u3cmin;
     double u3cmax;
     //Expressions variables
     //Delay declarations
//==This code has been autogenerated using Compgen==
     //inputVariables pointers
     double *mpphiref;
     double *mpthetaref;
     double *mppsiref;
     double *mpphimax;
     double *mpphi;
     double *mptheta;
     double *mppsi;
     double *mpbeta;
     double *mpQb;
     double *mpRb;
     double *mpUb;
     //inputParameters pointers
     double *mpKphi;
     double *mpKphipsi;
     double *mpKelev;
     double *mpKdelev;
     double *mpKrud;
     double *mpKdrud;
     double *mpu1min;
     double *mpu1max;
     double *mpu2min;
     double *mpu2max;
     double *mpu3min;
     double *mpu3max;
     double *mpU0;
     //outputVariables pointers
     double *mpuaerL;
     double *mpuaerR;
     double *mpuelev;
     double *mpurud;
     EquationSystemSolver *mpSolver;

public:
     static Component *Creator()
     {
        return new SignalAttitudeControl();
     }

     void configure()
     {
//==This code has been autogenerated using Compgen==

        mNstep=9;

        //Add ports to the component
        //Add inputVariables to the component
            addInputVariable("phiref","Reference signal \
roll","rad",0.,&mpphiref);
            addInputVariable("thetaref","Reference signal \
tip","rad",0.,&mpthetaref);
            addInputVariable("psiref","Reference signal \
yaw","rad",0.,&mppsiref);
            addInputVariable("phimax","Maximum bank angle for \
turn","rad",1.,&mpphimax);
            addInputVariable("phi","roll angle","rad",0.,&mpphi);
            addInputVariable("theta","tipp angle","rad",0.,&mptheta);
            addInputVariable("psi","yaw angle","rad",0.,&mppsi);
            addInputVariable("beta","side slip angle","rad",0.,&mpbeta);
            addInputVariable("Qb","tip angle rate","rad/s",0.,&mpQb);
            addInputVariable("Rb","yaw angle rate","rad/s",0.,&mpRb);
            addInputVariable("Ub","actual speed","m/s",0.,&mpUb);

        //Add inputParammeters to the component
            addInputVariable("Kphi", "Gain roll", "rad", 3.,&mpKphi);
            addInputVariable("Kphipsi", "Gain yaw/roll", "rad", \
2.,&mpKphipsi);
            addInputVariable("Kelev", "Gain tip, default", "rad", \
4.,&mpKelev);
            addInputVariable("Kdelev", "Gain tip, default", "rad", \
1.,&mpKdelev);
            addInputVariable("Krud", "Gain yaw, default", "rad", 1.,&mpKrud);
            addInputVariable("Kdrud", "Gain yaw rate, default", "", \
1.,&mpKdrud);
            addInputVariable("u1min", "Minium output signal roll", "rad", \
-0.9,&mpu1min);
            addInputVariable("u1max", "Maximum output signal roll", "rad", \
0.9,&mpu1max);
            addInputVariable("u2min", "Minium output signal tip", "rad", \
-0.7,&mpu2min);
            addInputVariable("u2max", "Maximum output signal tip", "rad", \
0.7,&mpu2max);
            addInputVariable("u3min", "Minium output signal yaw", "rad", \
-0.7,&mpu3min);
            addInputVariable("u3max", "Maximum output signal yaw", "rad", \
0.7,&mpu3max);
            addInputVariable("U0", "Reference speed for compensation", "m/s", \
100.,&mpU0);
        //Add outputVariables to the component
            addOutputVariable("uaerL","left aerleron","rad",0.,&mpuaerL);
            addOutputVariable("uaerR","right aerleron","rad",0.,&mpuaerR);
            addOutputVariable("uelev","elevator","rad",0.,&mpuelev);
            addOutputVariable("urud","rudder","rad",0.,&mpurud);

//==This code has been autogenerated using Compgen==
        //Add constantParameters
     }

    void initialize()
     {
        //Read port variable pointers from nodes

        //Read variables from nodes

        //Read inputVariables from nodes
        phiref = (*mpphiref);
        thetaref = (*mpthetaref);
        psiref = (*mppsiref);
        phimax = (*mpphimax);
        phi = (*mpphi);
        theta = (*mptheta);
        psi = (*mppsi);
        beta = (*mpbeta);
        Qb = (*mpQb);
        Rb = (*mpRb);
        Ub = (*mpUb);

        //Read inputParameters from nodes
        Kphi = (*mpKphi);
        Kphipsi = (*mpKphipsi);
        Kelev = (*mpKelev);
        Kdelev = (*mpKdelev);
        Krud = (*mpKrud);
        Kdrud = (*mpKdrud);
        u1min = (*mpu1min);
        u1max = (*mpu1max);
        u2min = (*mpu2min);
        u2max = (*mpu2max);
        u3min = (*mpu3min);
        u3max = (*mpu3max);
        U0 = (*mpU0);

        //Read outputVariables from nodes
        uaerL = (*mpuaerL);
        uaerR = (*mpuaerR);
        uelev = (*mpuelev);
        urud = (*mpurud);

//==This code has been autogenerated using Compgen==

        //LocalExpressions
        Kv = Power(U0,2)/(Power(U0,2) + Power(Abs(Ub),2));
        u1cmin = Kv*u1min;
        u1cmax = Kv*u1max;
        u2cmin = Kv*u2min;
        u2cmax = Kv*u2max;
        u3cmin = u3min;
        u3cmax = u3max;

        //Initialize delays


        simulateOneTimestep();

     }
    void simulateOneTimestep()
     {
        //Read variables from nodes

        //Read inputVariables from nodes
        phiref = (*mpphiref);
        thetaref = (*mpthetaref);
        psiref = (*mppsiref);
        phimax = (*mpphimax);
        phi = (*mpphi);
        theta = (*mptheta);
        psi = (*mppsi);
        beta = (*mpbeta);
        Qb = (*mpQb);
        Rb = (*mpRb);
        Ub = (*mpUb);

        //Read inputParameters from nodes
        Kphi = (*mpKphi);
        Kphipsi = (*mpKphipsi);
        Kelev = (*mpKelev);
        Kdelev = (*mpKdelev);
        Krud = (*mpKrud);
        Kdrud = (*mpKdrud);
        u1min = (*mpu1min);
        u1max = (*mpu1max);
        u2min = (*mpu2min);
        u2max = (*mpu2max);
        u3min = (*mpu3min);
        u3max = (*mpu3max);
        U0 = (*mpU0);

        //LocalExpressions
        Kv = Power(U0,2)/(Power(U0,2) + Power(Abs(Ub),2));
        u1cmin = Kv*u1min;
        u1cmax = Kv*u1max;
        u2cmin = Kv*u2min;
        u2cmax = Kv*u2max;
        u3cmin = u3min;
        u3cmax = u3max;

          //Expressions
          uaerL = limit(Kphi*Kv*(diffAngle(phiref,phi) + \
limit(Kphipsi*diffAngle(psiref,psi),-phimax,phimax)),u1cmin,u1cmax);
          uaerR = limit(-(Kphi*Kv*(diffAngle(phiref,phi) + \
limit(Kphipsi*diffAngle(psiref,psi),-phimax,phimax))),u1cmax,u1cmin);
          uelev = limit(-(Kv*(-(Kdelev*Qb) + \
Kelev*diffAngle(thetaref,theta))),u2cmin,u2cmax);
          urud = limit(-(Kv*(beta*Krud + Kdrud*Rb)),u3cmin,u3cmax);

        //Calculate the delayed parts


        //Write new values to nodes
        //outputVariables
        (*mpuaerL)=uaerL;
        (*mpuaerR)=uaerR;
        (*mpuelev)=uelev;
        (*mpurud)=urud;

        //Update the delayed variabels

     }
    void deconfigure()
    {
        delete mpSolver;
    }
};
#endif // SIGNALATTITUDECONTROL_HPP_INCLUDED
